home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / general / raytrace / radiance / simplerd.lha / simplerad / FinalFTP / Conv / Qmodel / MxMatrix.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-05-26  |  6.7 KB  |  409 lines
  1. /*
  2.  *    Program:    None
  3.  *    Module:        MxMatrix.c
  4.  *    Programmer:    John Buchanan
  5.  *
  6.  *    Description:
  7.  *            A variety of routines to handle matrices.
  8.  *    
  9.  */
  10.  
  11. /*                           Exported functions    */
  12. #define MX_N 4
  13. #include "MxMatrix.h"
  14. #include <math.h>
  15. #include <stdio.h>
  16. typedef float matrix[MX_N][MX_N];
  17. typedef float vector[MX_N];
  18. #define DAMN_SMALL 1.0e-10
  19.  
  20.  
  21. void MxCopy(a,b)    /* Copy a to b */
  22. matrix     a,b;
  23.     {
  24.     register int i,j;
  25.     for(i = 0; i < MX_N; i++)
  26.         for(j = 0; j < MX_N; j++)
  27.             b[i][j]=a[i][j];
  28.     }
  29. void MxTranspose(a,r)    /* Transpose a into result r */
  30. matrix a,r;
  31.     {
  32.     register int i,j;
  33.     matrix tmp;
  34.     for(i = 0; i < MX_N; i++)
  35.         for(j = 0; j < MX_N; j++)
  36.             tmp[j][i]=a[i][j];
  37.     MxCopy(tmp,r);
  38.     }
  39.  
  40.  
  41. void MxTranslate(a,x,y,z)
  42. matrix a;
  43. float x,y,z;
  44.     {
  45.     a[3][0] = x;
  46.     a[3][1] = y;
  47.     a[3][2] = z;
  48.     }
  49.  
  50. void MxScale(a,x,y,z)
  51. matrix a;
  52. float x,y,z;
  53.     {
  54.     a[0][0] = x;
  55.     a[1][1] = y;
  56.     a[2][2] = z;
  57.     }
  58.  
  59. void MxRotateD(a,degrees,axis)    /* Create rotation matrix result r */
  60. matrix a;
  61. float degrees;
  62. char axis;
  63.     {
  64.     MxRotateR(a,degrees/180.0*M_PI,axis);
  65.     }
  66.  
  67. void MxRotateR(a,radians,axis)    /* Create rotation matrix result r */
  68. matrix a;
  69. float radians;
  70. char axis;
  71.     {
  72.     float Cos,Sin;
  73.     Cos = cos(radians);
  74.     Sin = sin(radians);
  75.     switch(axis)
  76.         {
  77.         case 'x':
  78.         case 'X':
  79.             a[1][1] = Cos;
  80.             a[1][2] = Sin;
  81.             a[2][1] = -Sin;
  82.             a[2][2] = Cos;
  83.             break;
  84.         case 'y':
  85.         case 'Y':
  86.             a[0][0] = Cos;
  87.             a[0][2] = -Sin;
  88.             a[2][0] = Sin;
  89.             a[2][2] = Cos;
  90.             break;
  91.         case 'z':
  92.         case 'Z':
  93.             a[0][0] = Cos;
  94.             a[0][1] = Sin;
  95.             a[1][0] = -Sin;
  96.             a[1][1] = Cos;
  97.             break;
  98.         default:
  99.             break;
  100.         }
  101.     }
  102.  
  103.  
  104. void MxIdentity(a)    /* store the identity in a */
  105. matrix a;
  106.     {
  107.     register int i,j;
  108.     for(i = 0; i < MX_N; i++)
  109.         for(j = 0; j < MX_N; j++)
  110.             a[i][j] = (i == j ? 1.0 : 0.0);
  111.     }
  112. void MxScalarMultiply(s,a,r)     /* multiply a by scalar s, put result in r */
  113. float s;
  114. matrix a,r;
  115.     {
  116.     register int i,j;
  117.     for(i = 0; i < MX_N; i++)
  118.         for(j = 0; j < MX_N; j++)
  119.             r[i][j] = s * a[i][j];
  120.     }
  121. void MxVectorMultiply(p,a,q)    /* Multiply vector p by a into q */
  122. matrix a;
  123. vector p,q;
  124.     {
  125.     register int i,j;
  126.     vector qTemp;
  127.     for(i = 0; i < MX_N; i++)
  128.         {
  129.         qTemp[i] = 0.0;
  130.         for(j = 0; j < MX_N; j++)
  131.             qTemp[i] += p[j] * a[j][i];
  132.         }
  133.     VecCopy(qTemp,q);
  134.     }
  135. void MxNegate(a,r)        /* Negate a into r */
  136. matrix a,r;
  137.     {
  138.     register int i,j;
  139.     for(i = 0; i < MX_N; i++)
  140.         for(j = 0; j < MX_N; j++)
  141.             r[i][j] = - a[i][j];
  142.     }
  143. void MxAdd(a,b,r)    /* add a and b into r */
  144. matrix a,b,r;
  145.     {
  146.     register int i,j;
  147.     for(i = 0; i < MX_N; i++)
  148.         for(j = 0; j < MX_N; j++)
  149.             r[i][j] =  a[i][j] + b[i][j];
  150.     }
  151.  
  152. void MxSubstract(a,b,r)     /* substract b from a into r */
  153. matrix a,b,r;
  154.     {
  155.     register int i,j;
  156.     for(i = 0; i < MX_N; i++)
  157.         for(j = 0; j < MX_N; j++)
  158.             r[i][j] =  a[i][j] - b[i][j];
  159.     }
  160.  
  161. void MxMultiply(a,b,r)    /* Multiply a by b put result in r */
  162. matrix a,b,r;
  163.     {
  164.     matrix tmp;
  165.     register int i,j,k;
  166.     for(i = 0; i < MX_N; i++)
  167.         for(j = 0; j < MX_N; j++)
  168.             {
  169.             tmp[i][j] = 0.0;
  170.             for(k = 0; k < MX_N; k++)
  171.                 tmp[i][j] +=  a[i][k] * b[k][j];
  172.             }
  173.     MxCopy(tmp,r);
  174.     }
  175. void MxDivide(a,b,r)
  176. matrix a,b,r;
  177.     {
  178.     }
  179. void MxReverseDivide(a,b,r)
  180. matrix a,b,r;
  181.     {
  182.     }
  183.  
  184. static char *MxOverFlow = "Mx Error: Stack overflow";
  185. static char *MxUnderFlow = "Mx Error: Stack underflow";
  186. static matrix MxStack[MX_STKLEN];
  187. static int MxStackTop = -1;
  188.  
  189. int MxStackInit()    /* initialize the stack */
  190.     {
  191.     MxStackTop = -1;
  192.     }
  193. char *MxPush(m)        /* Push matrix m onto stack */
  194. matrix m;
  195.     {
  196.     if(MxStackTop++ == MX_STKLEN)
  197.         {
  198.         MxStackTop--;
  199.         return (MxOverFlow);
  200.         }
  201.     MxCopy(m,MxStack[MxStackTop]);
  202.     return (NULL);
  203.     }
  204. char *MxPop(m)        /* place the matrix at the top of the stack in m */
  205. matrix m;
  206.     {
  207.     if(MxStackTop < 0)
  208.         return(MxUnderFlow);
  209.     MxCopy(MxStack[MxStackTop--],m);
  210.     return (NULL);
  211.     }
  212. float MxTranxform(a,b,m)
  213. vector a,b;
  214. matrix m;
  215.     {
  216.     }
  217. void MxPrint(m)
  218. matrix m;
  219.     {
  220.     register int i,j;
  221.     printf("matrix output\n");
  222.     for(i = 0; i < MX_N; i++)
  223.         {
  224.         for(j = 0; j < MX_N; j++)
  225.             printf("\t%g",m[i][j]);
  226.         printf("\n");
  227.         }
  228.     }
  229. void MxGet(m)
  230. matrix m;
  231.     {
  232.     register int i,j;
  233.     printf("Enter the matrix at the prompts\n");
  234.     for(i = 0; i < MX_N; i++)
  235.         for(j = 0; j < MX_N; j++)
  236.             {
  237.             printf("m[%d][%d]:> ",i,j);
  238.             scanf("%lf",&m[i][j]);
  239.             }
  240.     }
  241.     
  242.  
  243. void VecCopy(a,b)
  244. vector a,b;
  245.     {
  246.     register int i;
  247.     for(i=0;i<MX_N;i++)
  248.         b[i] = a[i];
  249.     }
  250.     
  251. void VecSubstract(a,b,c)
  252. vector a,b,c;
  253.     {
  254.     register int i;
  255.     for(i=0;i<MX_N;i++)
  256.         c[i] = a[i] - b[i];
  257.     
  258.     }
  259.  
  260.  
  261. void VecAdd(a,b,c)
  262. vector a,b,c;
  263.     {
  264.     register int i;
  265.     for(i=0;i<MX_N;i++)
  266.         c[i] = a[i] + b[i];
  267.     }
  268.  
  269. void VecCross(a,b,c)
  270. vector a,b,c;
  271.     {
  272.     register int i;
  273.     vector tmp;
  274.     tmp[0] = a[1]*b[2] - a[2]*b[1];
  275.     tmp[1] = -(a[0]*b[2] - a[2]*b[0]);
  276.     tmp[2] = a[0]*b[1] - a[1]*b[0];
  277.     tmp[3] = 0.0; /* for now */
  278.     VecCopy(tmp,c);
  279.     }
  280. void VecPrint(a)
  281. vector a;
  282.     {
  283.     register int i;
  284.     printf("vector output");
  285.     for(i=0; i< MX_N; i++)
  286.         printf("\t%g",a[i]);
  287.     printf("\n");
  288.     }
  289.     
  290. float VecDot(p,q)        /* Return dot product of p and q */
  291. vector p,q;
  292.     {
  293.     register int i;
  294.     float result = 0.0;
  295.     for (i=0; i< MX_N; i++)
  296.         result += p[i] * q[i];
  297.     return (result);
  298.     }
  299.     
  300. void VecNormalize(p)
  301. vector p;
  302.     {
  303.     register int i;
  304.     float length = 0.0;
  305.     for(i= 0; i< MX_N; i++)
  306.         length += p[i]*p[i];
  307.     if (length == 0.0)
  308.         {
  309.         fprintf(stderr,"zero length vector cannot be normalized\n");
  310.         return;
  311.         }
  312.     length = sqrt(length);
  313.     for(i= 0; i< MX_N; i++)
  314.         p[i] = p[i] / length;
  315.     }
  316. float VecLength(p)
  317. vector p;
  318.     {
  319.     register int i;
  320.     float length = 0.0;
  321.     for(i= 0; i< MX_N; i++)
  322.         length += p[i]*p[i];
  323.     return(sqrt(length));    
  324.     }
  325.     
  326. VecScalarMultiply(a,v,d)
  327. float a;
  328. vector v;
  329. vector d;
  330.     {
  331.     register int i;
  332.     for (i = 0; i< MX_N; i++)
  333.         d[i] = v[i] *a;
  334.     }
  335.  
  336.  
  337. static float MxElementMinor(a, i, j,order)
  338.     register matrix a;
  339.     register int i, j, order;
  340.     {
  341.     int k, l, ii, jj;
  342.     float sign,  MxDeterminant();
  343.     register matrix new;
  344.     sign = (((i+j) % 2 ) == 0 ) ? 1.0 : -1.0;
  345.     /* copy matrix into smaller */
  346.     k = 0;
  347.     for (ii=0; ii<order; ii++) {
  348.         if ( ii==i) continue;
  349.         l=0;
  350.         for (jj=0; jj<order; jj++) {
  351.             if (jj==j) continue;
  352.             new[k][l] = a[ii][jj];
  353.             l++;
  354.             };
  355.         k++;
  356.         };
  357.     return(sign*MxDeterminant(new, MX_N-1));
  358.     }
  359.  
  360. float MxInvert(a,b)
  361. matrix a,b;
  362.     {
  363.     float det;
  364.     float MxElementMinor(), MxDeterminant();
  365.     register matrix at;
  366.     int i, j;
  367.     det = MxDeterminant(a, MX_N);
  368.     if(det < DAMN_SMALL && det > -DAMN_SMALL)
  369.         return(det);
  370.     MxTranspose(a, at);
  371.     for (i=0; i< MX_N; i++) 
  372.         for(j=0; j<MX_N; j++) 
  373.             b[i][j] = MxElementMinor(at, i, j, MX_N) / det;
  374.     return(det);
  375.     }
  376.  
  377. float MxDeterminant ( a, order)
  378.     register matrix a;
  379.     int order;
  380.     {
  381.     float det, mux;
  382.     register matrix new;
  383.     int i, j, ii, jj, k, l;
  384.  
  385.     if( order ==1) return(a[0][0]);
  386.     det = 0.0;
  387.     mux = 1.0;
  388.     i = 0;
  389.     for (j=0; j<order; j++) {
  390.         /* copy matrix into smaller */
  391.         k = 0;
  392.         for (ii=0; ii<order; ii++) {
  393.             l=0;
  394.             if (ii==i) continue;
  395.             for (jj=0; jj<order; jj++) {
  396.                 if (jj==j) continue;
  397.                 new[k][l] = a[ii][jj];
  398.                 l++;
  399.                 };
  400.             k++;
  401.             };
  402.         det += mux*a[i][j]*MxDeterminant(new, order-1);
  403.         mux = -mux;
  404.         };
  405.     return(det);
  406. }
  407.  
  408.  
  409.